package com.crunch.math;

import java.util.Arrays;

/**
 * A class for 3x3 matrices.
 */
public final class Matrix33f {
	float[] d;

	/**
	 * Constructs an identity matrix.
	 */
	public Matrix33f() {
		d = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f };
	}

	/**
	 * Sets this matrix to the identity matrix.
	 */
	public void setIdentity() {
		d[0] = 1.0f;
		d[1] = 0.0f;
		d[2] = 0.0f;
		d[3] = 0.0f;
		d[4] = 1.0f;
		d[5] = 0.0f;
		d[6] = 0.0f;
		d[7] = 0.0f;
		d[8] = 1.0f;
	}

	/**
	 * Constructs a matrix by copying the one provided.
	 */
	public Matrix33f(Matrix33f m) {
		d = Arrays.copyOf(m.d, m.d.length);
	}

	/**
	 * Sets this matrix by copying the one provided.
	 */
	public void set(Matrix33f m) {
		System.arraycopy(m.d, 0, d, 0, d.length);
	}

	/**
	 * Constructs a matrix from the given elements, specified in row-major order.
	 */
	public Matrix33f(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
		d = new float[] { m00, m01, m02, m10, m11, m12, m20, m21, m22 };
	}

	/**
	 * Sets this matrix from the given elements, specified in row-major order.
	 */
	public void set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22) {
		d[0] = m00;
		d[1] = m01;
		d[2] = m02;
		d[3] = m10;
		d[4] = m11;
		d[5] = m12;
		d[6] = m20;
		d[7] = m21;
		d[8] = m22;
	}

	/**
	 * Returns the given element using row-major ordering.
	 */
	public float get(int i) {
		return d[i];
	}

	/**
	 * Sets the given element using row-major ordering.
	 */
	public void set(int i, float v) {
		d[i] = v;
	}

	/**
	 * Returns the element at the given row and column.
	 */
	public float get(int row, int column) {
		return d[row*3 + column];
	}

	/**
	 * Sets the element at the given row and column.
	 */
	public void set(int row, int column, float v) {
		d[row*3 + column] = v;
	}

	/**
	 * Returns the sum of this matrix and {@code rhs}.
	 */
	public Matrix33f add(Matrix33f rhs) {
		return new Matrix33f(this).addAssign(rhs);
	}

	/**
	 * Adds {@code rhs} to this matrix.
	 */
	public Matrix33f addAssign(Matrix33f rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] += rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns the difference between this matrix and {@code rhs}.
	 */
	public Matrix33f subtract(Matrix33f rhs) {
		return new Matrix33f(this).subtractAssign(rhs);
	}

	/**
	 * Subtracts {@code rhs} from this matrix.
	 */
	public Matrix33f subtractAssign(Matrix33f rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] -= rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns this matrix negated.
	 */
	public Matrix33f negate() {
		return new Matrix33f(this).negateAssign();
	}

	/**
	 * Negates this matrix.
	 */
	public Matrix33f negateAssign() {
		for (int i = 0; i < d.length; ++i) {
			d[i] = -d[i];
		}
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Vector3f multiply(Vector3f rhs) {
		Vector3f vec = new Vector3f();
		for (int y = 0; y < 3; ++y) {
			float v = 0.0f;
			for (int x = 0; x < 3; ++x) {
				v += get(y, x) * rhs.get(x);
			}
			vec.set(y, v);
		}
		return vec;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix33f multiply(Matrix33f rhs) {
		return new Matrix33f(this).multiplyAssign(rhs);
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix44af multiply(Matrix44af rhs) {
		return new Matrix44af(rhs).preMultiplyAssign(this);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix33f multiplyAssign(Matrix33f rhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 3; ++x) {
				int idx = y*3 + x;
				for (int i = 0; i < 3; ++i) {
					dt[idx] += get(y, i) * rhs.get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Pre-multiplies this matrix by {@code lhs}.
	 */
	public Matrix33f preMultiplyAssign(Matrix33f lhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 3; ++x) {
				int idx = y*3 + x;
				for (int i = 0; i < 3; ++i) {
					dt[idx] += lhs.get(y, i) * get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix33f multiply(float rhs) {
		return new Matrix33f(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix33f multiplyAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] *= rhs;
		}
		return this;
	}

	/**
	 * Returns the quotient of this matrix and {@code rhs}.
	 */
	public Matrix33f divide(float rhs) {
		return new Matrix33f(this).divideAssign(rhs);
	}

	/**
	 * Divides this matrix by {@code rhs}.
	 */
	public Matrix33f divideAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] /= rhs;
		}
		return this;
	}

	/**
	 * Returns whether this matrix is equal to the object {@code o}.
	 */
	@Override
	public boolean equals(Object o) {
		if (o == this) {
			return true;
		} else if (o == null || !(o instanceof Matrix33f)) {
			return false;
		} else {
			return equals((Matrix33f) o);
		}
	}

	/**
	 * Returns a hash code for this object.
	 */
	@Override
	public int hashCode() {
		int bits = 0;
		for (int i = 0; i < d.length; ++i) {
			bits ^= Float.floatToIntBits(d[i]);
		}
		return bits;
	}

	/**
	 * Returns whether this matrix is equal to the matrix {@code rhs}.
	 */
	public boolean equals(Matrix33f rhs) {
		return Arrays.equals(d, rhs.d);
	}

	/**
	 * Returns the transpose of this matrix.
	 */
	public Matrix33f transpose() {
		return new Matrix33f(this).transposeAssign();
	}

	/**
	 * Transposes this matrix.
	 */
	public Matrix33f transposeAssign() {
		float tmp = d[1];
		d[1] = d[2];
		d[2] = tmp;
		return this;
	}

	/**
	 * Returns the inverse of this matrix, or {@code null} if this matrix is not invertible.
	 */
	public Matrix33f invert() {
		return new Matrix33f(this).invertAssign();
	}

	/**
	 * Inverts this matrix, or returns {@code null} if this matrix is not invertible.
	 */
	public Matrix33f invertAssign() {
		float det = determinant();
		if (det == 0.0f) {
			return null;
		}

		float invDet = 1.0f / det;
		float[] dt = new float[] {
				(d[8]*d[4] - d[7]*d[5]) * invDet,
				(d[7]*d[2] - d[8]*d[1]) * invDet,
				(d[5]*d[1] - d[4]*d[2]) * invDet,
				(d[6]*d[5] - d[8]*d[3]) * invDet,
				(d[8]*d[0] - d[6]*d[2]) * invDet,
				(d[3]*d[2] - d[5]*d[0]) * invDet,
				(d[7]*d[3] - d[6]*d[4]) * invDet,
				(d[6]*d[1] - d[7]*d[0]) * invDet,
				(d[4]*d[0] - d[3]*d[1]) * invDet
		};
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians about the x-axis.
	 */
	public Matrix33f rotateX(float a) {
		return new Matrix33f(this).rotateXAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians about the x-axis.
	 */
	public Matrix33f rotateXAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);

		float r11 = cosA;
		float r12 = -sinA;
		float r21 = sinA;
		float r22 = cosA;

		float m10 = r11*d[3] + r12*d[6];
		float m11 = r11*d[4] + r12*d[7];
		float m12 = r11*d[5] + r12*d[8];

		float m20 = r21*d[3] + r22*d[6];
		float m21 = r21*d[4] + r22*d[7];
		float m22 = r21*d[5] + r22*d[8];

		d[3] = m10; d[4] = m11; d[5] = m12;
		d[6] = m20; d[7] = m21; d[8] = m22;

		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians about the y-axis.
	 */
	public Matrix33f rotateY(float a) {
		return new Matrix33f(this).rotateYAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians about the y-axis.
	 */
	public Matrix33f rotateYAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);

		float r00 = cosA;
		float r02 = sinA;
		float r20 = -sinA;
		float r22 = cosA;

		float m00 = r00*d[0] + r02*d[6];
		float m01 = r00*d[1] + r02*d[7];
		float m02 = r00*d[2] + r02*d[8];

		float m20 = r20*d[0] + r22*d[6];
		float m21 = r20*d[1] + r22*d[7];
		float m22 = r20*d[2] + r22*d[8];

		d[0] = m00; d[1] = m01; d[2] = m02;
		d[6] = m20; d[7] = m21; d[8] = m22;

		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians about the z-axis.
	 */
	public Matrix33f rotateZ(float a) {
		return new Matrix33f(this).rotateZAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians about the z-axis.
	 */
	public Matrix33f rotateZAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);

		float r00 = cosA;
		float r01 = -sinA;
		float r10 = sinA;
		float r11 = cosA;

		float m00 = r00*d[0] + r01*d[3];
		float m01 = r00*d[1] + r01*d[4];
		float m02 = r00*d[2] + r01*d[5];

		float m10 = r10*d[0] + r11*d[3];
		float m11 = r10*d[1] + r11*d[4];
		float m12 = r10*d[2] + r11*d[5];

		d[0] = m00; d[1] = m01; d[2] = m02;
		d[3] = m10; d[4] = m11; d[5] = m12;

		return this;
	}

	/**
	 * Returns this matrix rotated about the given axis by the given angle.
	 */
	public Matrix33f rotate(AxisAngle aa) {
		return new Matrix33f(this).rotateAssign(aa);
	}

	/**
	 * Rotates this matrix about the given axis by the given angle.
	 */
	public Matrix33f rotateAssign(AxisAngle aa) {
		Vector3f axis = aa.axis;
		float sinA = (float) Math.sin(aa.angle);
		float cosA = (float) Math.cos(aa.angle);
		float xy = axis.x()*axis.y();
		float xz = axis.x()*axis.z();
		float yz = axis.y()*axis.z();

		Matrix33f rot = new Matrix33f(
			cosA + axis.x()*axis.x()*(1 - cosA),
			xy*(1 - cosA) - axis.z()*sinA,
			xz*(1 - cosA) + axis.y()*sinA,
			xy*(1 - cosA) + axis.z()*sinA,
			cosA + axis.y()*axis.y()*(1 - cosA),
			yz*(1 - cosA) - axis.x()*sinA,
			xz*(1 - cosA) - axis.y()*sinA,
			yz*(1 - cosA) - axis.x()*sinA,
			cosA + axis.z()*axis.z()*(1 - cosA));
		return preMultiplyAssign(rot);
	}

	/**
	 * Returns this matrix rotated by the given quaternion.
	 */
	public Matrix33f rotate(Quaternionf q) {
		return new Matrix33f(this).rotateAssign(q);
	}

	/**
	 * Rotates this matrix by the given quaternion.
	 */
	public Matrix33f rotateAssign(Quaternionf q) {
		float i2 = 2.0f*q.i();
		float j2 = 2.0f*q.j();
		float k2 = 2.0f*q.k();

		Matrix33f rot = new Matrix33f(
				i2*q.i(),
				i2*q.j(),
				i2*q.k(),
				i2*q.r(),
				j2*q.j(),
				j2*q.k(),
				j2*q.r(),
				k2*q.k(),
				k2*q.r());
		return preMultiplyAssign(rot);
	}

	/**
	 * Returns this matrix scaled by {@code s}.
	 */
	public Matrix33f scale(Vector3f s) {
		return new Matrix33f(this).scaleAssign(s);
	}

	/**
	 * Scales this matrix by {@code s}.
	 */
	public Matrix33f scaleAssign(Vector3f s) {
		d[0] *= s.get(0);
		d[1] *= s.get(0);
		d[2] *= s.get(0);
		d[3] *= s.get(1);
		d[4] *= s.get(1);
		d[5] *= s.get(1);
		d[6] *= s.get(2);
		d[7] *= s.get(2);
		d[8] *= s.get(2);
		return this;
	}

	/**
	 * Returns the determinant of this matrix.
	 */
	public float determinant() {
		return d[0]*(d[8]*d[4] - d[7]*d[5]) + d[3]*(d[7]*d[2] - d[8]*d[1]) + d[6]*(d[5]*d[1] - d[4]*d[2]);
	}

	/**
	 * Returns the trace of this matrix.
	 */
	public float trace() {
		return d[0] + d[4] + d[8];
	}

	/**
	 * Returns the raw underlying mutable array.
	 */
	public float[] getArray() {
		return d;
	}

	/**
	 * Converts this matrix to a 4x4 matrix suitable for graphics transformations.
	 */
	public Matrix44f getGraphicsMatrix() {
		return new Matrix44f(
				d[0], d[1], d[2], 0.0f,
				d[3], d[4], d[5], 0.0f,
				d[6], d[7], d[8], 0.0f,
				0.0f, 0.0f, 0.0f, 1.0f);
	}
}
